#include "toroidal.h"
#include "stdlib.h"
#include <stdio.h>
#include <math.h>

fiq::fiq(double* a, double* b, int n)
{
    this->n = n;
    this->f_inphase = new double[Nx*Ny];
    this->f_quadrature = new double[Nx*Ny]; 
    for(int i = 0; i < Nx*Ny; i++)
    {
        f_inphase[i] = a[i];
        f_quadrature[i] = b[i];
    }
}

fiq::fiq(float* a, float* b, int n)
{
    this->n = n;
    this->f_inphase = new double[Nx*Ny];
    this->f_quadrature = new double[Nx*Ny]; 
    for(int i = 0; i < Nx*Ny; i++)
    {
        f_inphase[i] = a[i];
        f_quadrature[i] = b[i];
    }
}

fiq::fiq(int n)
{
    this->n = n;
    this->f_inphase = new double[Nx*Ny];
    this->f_quadrature = new double[Nx*Ny]; 
}

fiq::fiq(field* a, field* b, int n)
{
    this->n = n;
    this->f_inphase = new double[Nx*Ny];
    this->f_quadrature = new double[Nx*Ny]; 
    double* pa = a->getdata();
    double* pb = b->getdata();
    for(int i = 0; i < Nx*Ny; i++)
    {
        f_inphase[i] = pa[i];
        f_quadrature[i] = pb[i];
    }
}

double* fiq::get_fi()
{
    return this->f_inphase;
}

double* fiq::get_fq()
{
    return this->f_quadrature;
}

double fiq::interp(cylv vp)
{
    double ret1 = 0.0;
    double ret2 = 0.0;
    double r = vp.r;
    double z = vp.z;
    int rg = (int)((r - Rmin)/dr);
    int zg = (int)((z - Zmin)/dz);
    double ofr = (r - (double)rg * dr - Rmin)/dr;
    double ofz = (z - (double)zg * dz - Zmin)/dz;
    double ret = 0.0;
    double coefrs[4] = {ofr*(ofr-1.0)*(ofr-2.0)/-6.0, (ofr+1.0)*(ofr-1.0)*(ofr-2.0)/2.0,
    (ofr+1.0)*ofr*(ofr-2.0)/-2.0, (ofr+1.0)*ofr*(ofr-1.0)/6.0};
    double coefzs[4] = {ofz*(ofz-1.0)*(ofz-2.0)/-6.0, (ofz+1.0)*(ofz-1.0)*(ofz-2.0)/2.0,
    (ofz+1.0)*ofz*(ofz-2.0)/-2.0, (ofz+1.0)*ofz*(ofz-1.0)/6.0};
    for(int i = 0; i < 4; i++)
        for(int j = 0; j < 4; j++)
        {
            int point = 0;
            if(rg+i-1 < 0)
                point += Ny*(rg+i);
            else if(rg+i-1 > Nx)
                point += Ny*(rg+i-2);
            else
                point += Ny*(rg+i-1);
            if (zg+j-1 < 0)
                point += zg+j;
            else if (zg+j-1 > Nx)
                point += zg+j-2;
            else
                point += zg+j-1;
            ret1 += coefrs[i]*coefzs[j]*f_inphase[point];
            ret2 += coefrs[i]*coefzs[j]*f_quadrature[point];
        }
    return ret1*cos(n*vp.p) + ret2*sin(n*vp.p);
}

void fiq::partialphi(fiq* pp)
{
    for (int i = 0; i < Nx*Ny; i++)
    {
        pp->get_fi()[i] = this->f_quadrature[i]*n;
        pp->get_fq()[i] = -this->f_inphase[i]*n;
    }
}

void fiq::gradient(vfiq* grad)
{
    double *ppi = grad->get_ifp();
    double *ppq = grad->get_qfp();

    partialx(field(this->f_inphase), grad->get_ifr());
    partialy(field(this->f_inphase), grad->get_ifz());
    partialx(field(this->f_quadrature), grad->get_qfr());
    partialy(field(this->f_quadrature), grad->get_qfz());
    
    for (int i = 0; i < Nx*Ny; i++)
    {
        double R = (i/Ny)*dr + Rmin;
        ppi[i] = n*f_quadrature[i]/R;
        ppq[i] = -n*f_inphase[i]/R;
    }
}

vfiq::vfiq(int n)
{
    this->ifr = new double[Nx*Ny];
    this->ifz = new double[Nx*Ny];
    this->ifp = new double[Nx*Ny];
    this->qfr = new double[Nx*Ny];
    this->qfz = new double[Nx*Ny];
    this->qfp = new double[Nx*Ny];
    this->n = n;
}

vfiq::vfiq(vfield* a, vfield* b, int n)
{
    this->ifr = new double[Nx*Ny];
    this->ifz = new double[Nx*Ny];
    this->ifp = new double[Nx*Ny];
    this->qfr = new double[Nx*Ny];
    this->qfz = new double[Nx*Ny];
    this->qfp = new double[Nx*Ny];

    double* afr = a->getfr();
    double* afz = a->getfz();
    double* afp = a->getfphi();
    double* bfr = b->getfr();
    double* bfz = b->getfz();
    double* bfp = b->getfphi();
    for (int i = 0; i < Nx*Ny; i++)
    {
        ifr[i] = afr[i];
        ifz[i] = afz[i];
        ifp[i] = afp[i];
        qfr[i] = bfr[i];
        qfz[i] = bfz[i];
        qfp[i] = bfp[i];
    }
    this->n = n;
}

void vfiq::curl(vfiq* vfiqp)
{
    double *icR = vfiqp->get_ifr();
    double *icZ = vfiqp->get_ifz();
    double *icp = vfiqp->get_ifp();
    double *qcR = vfiqp->get_qfr();
    double *qcZ = vfiqp->get_qfz();
    double *qcp = vfiqp->get_qfp();

    double* prfzi = new double[Nx*Ny];
    double* pzfri = new double[Nx*Ny];
    double* pzfpi = new double[Nx*Ny];
    double* prfpi = new double[Nx*Ny];

    double* prfzq = new double[Nx*Ny];
    double* pzfrq = new double[Nx*Ny];
    double* pzfpq = new double[Nx*Ny];
    double* prfpq = new double[Nx*Ny];

    partialx(field(ifz),prfzi);
    partialy(field(ifr),pzfri);
    partialy(field(ifp),pzfpi);
    partialx(field(ifp),prfpi);

    partialx(field(qfz),prfzq);
    partialy(field(qfr),pzfrq);
    partialy(field(qfp),pzfpq);
    partialx(field(qfp),prfpq);
    for (int i = 0; i<Nx*Ny; i++)
    {
        double R = (i/Ny)*dr + Rmin;
        icp[i] = -prfzi[i]+pzfri[i];
        icR[i] = -pzfpi[i]+n*qfz[i]/R;
        icZ[i] = +prfpi[i] + ifp[i]/R - n*qfr[i]/R;

        qcp[i] = -prfzq[i] + pzfrq[i];
        qcR[i] = -pzfpq[i] - n*ifz[i]/R;
        qcZ[i] = +prfpq[i] + qfp[i]/R + n*ifr[i]/R;
    }
    return;
}

void vfiq::divergence(fiq* fiqp)
{
    double *id = fiqp->get_fi();
    double *qd = fiqp->get_fq();

    double* prfri = new double[Nx*Ny];
    double* pzfzi = new double[Nx*Ny];
    double* prfrq = new double[Nx*Ny];
    double* pzfzq = new double[Nx*Ny];

    partialx(field(ifr), prfri);
    partialy(field(ifz), pzfzi);
    partialx(field(qfr), prfrq);
    partialy(field(qfz), pzfzq);

    for (int i = 0; i<Nx*Ny; i++)
    {
        double R = (i/Ny)*dr + Rmin;
        id[i] = prfri[i]+pzfzi[i]+(ifr[i]+n*qfp[i])/R;
        qd[i] = prfri[i]+pzfzq[i]+(qfr[i]-n*ifp[i])/R;
    }
    return;
}

vec vfiq::interp(cylv vp)
{
    vec reti = {0.0, 0.0, 0.0};
    vec retq = {0.0, 0.0, 0.0};
    double r = vp.r;
    double z = vp.z;
    int rg = (int)((r - Rmin)/dr);
    int zg = (int)((z - Zmin)/dz);
    double ofr = (r - (double)rg * dr - Rmin)/dr;
    double ofz = (z - (double)zg * dz - Zmin)/dz;
    double ret = 0.0;
    double coefrs[4] = {ofr*(ofr-1.0)*(ofr-2.0)/-6.0, (ofr+1.0)*(ofr-1.0)*(ofr-2.0)/2.0,
    (ofr+1.0)*ofr*(ofr-2.0)/-2.0, (ofr+1.0)*ofr*(ofr-1.0)/6.0};
    double coefzs[4] = {ofz*(ofz-1.0)*(ofz-2.0)/-6.0, (ofz+1.0)*(ofz-1.0)*(ofz-2.0)/2.0,
    (ofz+1.0)*ofz*(ofz-2.0)/-2.0, (ofz+1.0)*ofz*(ofz-1.0)/6.0};
    for(int i = 0; i < 4; i++)
        for(int j = 0; j < 4; j++)
        {
            int point = 0;
            if(rg+i-1 < 0)
                point += Ny*(rg+i);
            else if(rg+i-1 > Nx)
                point += Ny*(rg+i-2);
            else
                point += Ny*(rg+i-1);
            if (zg+j-1 < 0)
                point += zg+j;
            else if (zg+j-1 > Nx)
                point += zg+j-2;
            else
                point += zg+j-1;
            reti.x += coefrs[i]*coefzs[j]*ifr[point];
            reti.y += coefrs[i]*coefzs[j]*ifp[point];
            reti.z += coefrs[i]*coefzs[j]*ifz[point];
            retq.x += coefrs[i]*coefzs[j]*qfr[point];
            retq.y += coefrs[i]*coefzs[j]*qfp[point];
            retq.z += coefrs[i]*coefzs[j]*qfz[point];
        }
    // rotation needs to be considered
    return cos(n*vp.p)*reti + sin(n*vp.p)*retq;
}
